#include "battery/battery.hpp"
#include "battery/charge.hpp"
#include "agv_msgs/msg/set_stop_bits.hpp"
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

class BatteryNode : public rclcpp::Node
{
private:
    rclcpp::Publisher<agv_msgs::msg::SetStopBits>::SharedPtr set_stop_pub_;
    rclcpp::Publisher<agv_msgs::msg::BatInfo>::SharedPtr battery_info_pub_;
    CBattery bat_;
    ChargeController charger;
    CEventManager event_manager_;
    int full_level_;
    int timeout_;
    
    rclcpp::TimerBase::SharedPtr timer_;
    
private:
    bool TimeON(bool start, unsigned long duration_ms);
    
public:
    BatteryNode();
    ~BatteryNode();
    void init();
    void update();
    bool chargingCheck();
    void set_stop_bits();
};

BatteryNode::BatteryNode() : Node("battery_node")
{
}

BatteryNode::~BatteryNode()
{
}

void BatteryNode::init()
{
    this->declare_parameter("/battery/full_level", 80);
    this->get_parameter("/battery/full_level", full_level_);
    RCLCPP_INFO(this->get_logger(), "/battery/full_level: %d", full_level_);
    
    this->declare_parameter("/battery/charger_timeout_sec", 30);
    this->get_parameter("/battery/charger_timeout_sec", timeout_);
    RCLCPP_INFO(this->get_logger(), "/battery/charger_timeout_sec: %d", timeout_);
    
    set_stop_pub_ = this->create_publisher<agv_msgs::msg::SetStopBits>("/set_stop_bits", 10);
    battery_info_pub_ = this->create_publisher<agv_msgs::msg::BatInfo>("batteryInfo", 1);
    
    bat_.init(shared_from_this());
    charger.init(shared_from_this());
    event_manager_.init(shared_from_this());
    
    timer_ = this->create_wall_timer(100ms, std::bind(&BatteryNode::update, this));
    
    RCLCPP_DEBUG(this->get_logger(), "[batteryManager] init OK ...");
}

void BatteryNode::update()
{
    static bool state_last(false), finished(false);
    
    if (bat_.batteryInfo_update())
    {
        bool charging = bat_.battery_info_.current > 0.0f;

        if (state_last != charging) {
            finished = (bat_.battery_info_.numeric_bat >= full_level_) &&
                       (bat_.battery_info_.current <= 0.0f) &&
                       (charger.get_state() != ChargeController::IDLE) &&
                       state_last;
            state_last = charging;
        }
        
        bat_.battery_info_.charge_finished = finished && (charger.get_state() != ChargeController::IDLE);
        bat_.battery_info_.charge_interrupted = chargingCheck();
        
        battery_info_pub_->publish(bat_.battery_info_);
        
        if (charger.get_state() == ChargeController::IDLE)
        {
            finished = false;
        }

        if (charger.OnChargePoint() && 
            bat_.battery_info_.numeric_bat <= full_level_ && 
            charger.get_state() == ChargeController::IDLE)
        {
            charger.set_charge(true);
        }
        
        if (!charger.OnChargePoint() || finished)
        {
            charger.set_charge(false);
        }
        
        set_stop_bits();
    }
}

bool BatteryNode::TimeON(bool start, unsigned long duration_ms)
{
    static auto last_time = std::chrono::steady_clock::now();
    static std::chrono::milliseconds accumulated_time(0);
    
    auto now = std::chrono::steady_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
    last_time = now;
    
    if (start) {
        accumulated_time += elapsed;
    } else {
        accumulated_time = std::chrono::milliseconds(0);
    }
    
    return accumulated_time.count() > duration_ms;
}

bool BatteryNode::chargingCheck()
{
    // 超时检查
    bool start = (bat_.battery_info_.numeric_bat < full_level_) &&
                 (bat_.battery_info_.current < 0.0f) &&
                 (charger.get_state() != ChargeController::IDLE);
    
    bool timeout = TimeON(start, timeout_ * 1000);
    
    if (timeout) {
        event_manager_.updateEvent(agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT,
                                  agv_msgs::msg::AGVEvent::SLOWDONE,
                                  agv_msgs::msg::AGVEvent::WARNING,
                                  "[alarm!] charge process have no current up to 30s",
                                  0,
                                  0);
    }
    else if (event_manager_.checkEvent(agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT)) {
        event_manager_.removeEvent(agv_msgs::msg::AGVEvent::CHARGE_FAIL_EVENT);
    }
    
    return timeout;
}

void BatteryNode::set_stop_bits()
{
    auto setStop = agv_msgs::msg::SetStopBits();
    
    if (charger.get_state() != ChargeController::IDLE) {
        setStop.name = "charging";
        setStop.stop = charger.get_state();
        set_stop_pub_->publish(setStop);
    }
    else if (bat_.battery_info_.charge_flag && charger.isManual()) {
        // 电池充电电流持续5s以上会发布停止标志位，加5s TP定时器过滤，防止出现车辆减速过程反向充电造成停车
        setStop.name = "manual_charging";
        setStop.stop = 1;
        set_stop_pub_->publish(setStop);
    }
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<BatteryNode>();
    node->init();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}